/*
 * sk_polygon.c
 *
 *  Created on: Feb 28, 2011
 *      Author: asish
 */
#include <stdio.h>
#include <cv.h>
#include "common.h"
#include "sk_polygon.h"
#include "sk_util.h"
#include "highgui.h"

static CvPoint2D32f g_polygonPoints[4];
static int g_noOfPoints;
static double sk_angle( CvPoint* pt1, CvPoint* pt2, CvPoint* pt0 );
static CvSeq* sk_findObject( IplImage* img, CvMemStorage* storage,CvPoint *pointA,CvPoint *pointB  );
static void sk_drawSquares( IplImage* img, CvSeq* squares );

static int thresh = 50;
CvMemStorage* storage = 0;

// helper function:
// finds a cosine of angle between vectors
// from pt0->pt1 and from pt0->pt2
static double
sk_angle( CvPoint* pt1, CvPoint* pt2, CvPoint* pt0 )
{
	double dx1 = pt1->x - pt0->x;
	double dy1 = pt1->y - pt0->y;
	double dx2 = pt2->x - pt0->x;
	double dy2 = pt2->y - pt0->y;
	return (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);
}

/*
 * Finds the rectangular object in the image which is bounded by the rectangular re
 */
static CvSeq*
sk_findObject( IplImage* img, CvMemStorage* storage, CvPoint *pointA,CvPoint *pointB  )
{
	CvSeq* contours;
	int i, c, l, N = 11;
	CvSize sz = cvSize( img->width & -2, img->height & -2 );
	IplImage *smoothedImage;
	IplImage* timg; // make a copy of input image
	IplImage* gray = cvCreateImage( sz, 8, 1 );
	IplImage* pyr = cvCreateImage( cvSize(sz.width/2, sz.height/2), 8, 3 );
	IplImage* tgray;
	CvSeq* result;
	double s, t;

	smoothedImage = SK_SmoothImage(img, CV_GAUSSIAN, 3, 3, 0, 0);
	timg = cvCloneImage( smoothedImage );

	// create empty sequence that will contain points -
	// 4 points per square (the square's vertices)
	CvSeq* squares = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvPoint), storage );

	// select the maximum ROI in the image
	// with the width and height divisible by 2
	cvSetImageROI( timg, cvRect( 0, 0, sz.width, sz.height ));


	// down-scale and upscale the image to filter out the noise
	cvPyrDown( timg, pyr, 7 );
	cvPyrUp( pyr, timg, 7 );
	tgray = cvCreateImage( sz, 8, 1 );
	g_noOfPoints = 0;

	// extract the 3rd color plane
	cvSetImageCOI( timg, 3 );
	cvCopy( timg, tgray, 0 );

	cvThreshold( tgray, gray, (5)*255/11, 255, CV_THRESH_TOZERO  );
	//cvWaitKey(20);


	// find contours and store them all as a list
	cvSetImageROI(gray,cvRect(pointA->x,pointA->y,pointB->x - pointA->x,pointB->y - pointA->y));
	cvFindContours( gray, storage, &contours, sizeof(CvContour),
			CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, cvPoint(pointA->x,pointA->y) );
	cvResetImageROI(gray);

	// test each contour
	while( contours )
	{
		// approximate contour with accuracy proportional
		// to the contour perimeter.
		// Approximate iteratively.
		result = cvApproxPoly( contours, sizeof(CvContour), storage,
				CV_POLY_APPROX_DP, cvContourPerimeter(contours)*0.02, 0 );
		result = cvApproxPoly( result, sizeof(CvContour), storage,
				CV_POLY_APPROX_DP, cvContourPerimeter(result)*0.03, 0 );
		result = cvApproxPoly( result, sizeof(CvContour), storage,
				CV_POLY_APPROX_DP, cvContourPerimeter(result)*0.04, 0 );

		if( result->total == 4 &&
				cvContourArea(result,CV_WHOLE_SEQ,0) > 1000 &&
				cvCheckContourConvexity(result) )
		{
			int flag = 0;
			s = 0;
			for (i =0 ; i < 5; ++i)
			{
				CvPoint *p;
				p = (CvPoint *)cvGetSeqElem(result,i);
				if (!((p->x > pointA->x && p->x < pointB->x) &&
						(p->y > pointA->y && p->y < pointB->y)))
				{
					flag = 1;
					break;
				}
				else
				{
					/* Do Nothing*/
				}

			}
			if (flag == 1)
			{
				flag = 0;
				contours = contours->h_next;
				continue;
			}
			for( i = 0; i < 5; i++ )
			{
				// find minimum angle between joint
				// edges (maximum of cosine)
				if( i >= 2 )
				{
					t = fabs(sk_angle(
							(CvPoint*)cvGetSeqElem( result, i ),
							(CvPoint*)cvGetSeqElem( result, i-2 ),
							(CvPoint*)cvGetSeqElem( result, i-1 )));
					s = s > t ? s : t;
				}
			}
			// if cosines of all angles are small
			// (all angles are ~90 degree) then write quandrange
			// vertices to resultant sequence
			if( s <= 0.3 )
			{
				CvPoint p;
				for( i = 0; i < 4; i++ )
				{
					cvSeqPush( squares,
							(CvPoint*)cvGetSeqElem( result, i ));
					memcpy(&p, (CvPoint*)cvGetSeqElem( result, i),sizeof(CvPoint));
					g_polygonPoints[i] = cvPointTo32f(p);

				}

				g_noOfPoints = result->total;

			}
		}

		// take the next contour
		contours = contours->h_next;
	}


	// release all the temporary images
	cvReleaseImage( &gray );
	cvReleaseImage( &pyr );
	cvReleaseImage( &tgray );
	cvReleaseImage( &timg );
	return squares;
}

CvSeq *
SK_ApproxPoly(CvSeq *l_seq)
{
	CvMemStorage* l_storage = NULL;
	CvSeq *result;
	cvCreateMemStorage(0);

	result = cvApproxPoly( l_seq, sizeof(CvContour), l_storage,
			CV_POLY_APPROX_DP, 10, 0 );
	cvReleaseMemStorage(&l_storage);
	return result;
}

// the function draws all the squares in the image
void
sk_drawSquares( IplImage* img, CvSeq* squares)
{
	CvSeqReader reader;
	IplImage* cpy = cvCloneImage( img );
	int i;

	// initialize reader of the sequence
	cvStartReadSeq( squares, &reader, 0 );

	// read 4 sequence elements at a time (all vertices of a square)
	for( i = 0; i < squares->total; i += 4 )
	{
		CvPoint pt[4], *rect = pt;
		int count = 4;

		// read 4 vertices
		CV_READ_SEQ_ELEM( pt[0], reader );
		CV_READ_SEQ_ELEM( pt[1], reader );
		CV_READ_SEQ_ELEM( pt[2], reader );
		CV_READ_SEQ_ELEM( pt[3], reader );
		// draw the square as a closed polyline
		cvPolyLine( cpy, &rect, &count, 1, 1, CV_RGB(0,255,0), 3, CV_AA, 0 );
	}

	// show the resultant image
	cvNamedWindow("Test",CV_WINDOW_AUTOSIZE);
	cvShowImage( "Input", cpy );
	cvShowImage( "Test", cpy );
	cvReleaseImage( &cpy );
}

/*
 * Initialize the polygon by storing the initial points and finding the object.
 */

int
SK_InitPolygon(IplImage *l_p_image, CvPoint *pointA, CvPoint *pointB, CvPoint2D32f **polyPoints)
{
	int retVal;
	storage = cvCreateMemStorage(0);
	sk_findObject( l_p_image, storage, pointA, pointB );
	*polyPoints = g_polygonPoints;
	cvReleaseMemStorage(&storage);
	retVal = g_noOfPoints;
	return retVal;
}

static int
sk_DetectPolygons(IplImage *l_p_image)
{
	IplImage *l_thresholdedImage;
	int noOfContours;
	CvSeq *contour;
	CvMemStorage* storage;
	int l_noOfPoints = 0;
	IplImage *tempImg = cvCreateImage( cvGetSize(l_p_image), 8, 1 );

	cvCvtColor( l_p_image, tempImg, CV_BGR2GRAY );
	l_thresholdedImage = SK_DoThreshold( tempImg, 50,255,CV_THRESH_BINARY);
	storage = cvCreateMemStorage(0);
	noOfContours = cvFindContours( l_thresholdedImage, storage, &contour, sizeof(CvContour),
			CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0) );
	//printf("No of contours = %d\n",noOfContours);
	if (noOfContours == 1)
	{
		cvApproxPoly( contour, sizeof(CvContour), storage, CV_POLY_APPROX_DP, 5, 1 );
		//if (NULL != g_polygonPoints)
		//	free(g_polygonPoints);
		//g_polygonPoints = (CvPoint2D32f *)malloc((contour->total)*sizeof(CvPoint2D32f));
		cvCvtSeqToArray(contour,g_polygonPoints,CV_WHOLE_SEQ);
		l_noOfPoints = contour->total;
		//printf("No of points = %d\n",l_noOfPoints);
	}
	else
	{
		//TODO: Need some extra processing.
		printf("More than one polygon detected. !!\n ");
	}
	cvReleaseImage(&tempImg);
	cvReleaseImage(&l_thresholdedImage);
	return l_noOfPoints;
}
